#include "bsp/bsp.h"
#include "controller.h"
#include "mc_config.h"

void contrl_init(controller_t *ctrl) {
    memset(ctrl, 0, sizeof(controller_t));
    ctrl->mode_req = ctrl->mode_running = CTRL_MODE_OPEN;
    ctrl->foc.ts = mc_conf()->ts;
	ctrl->foc.pi_curr_id.kp = mc_conf()->c.pid[pid_id].kp;
	ctrl->foc.pi_curr_id.ki = mc_conf()->c.pid[pid_id].ki;
	ctrl->foc.pi_curr_id.ts = mc_conf()->ts;
	ctrl->foc.pi_curr_iq.kp = mc_conf()->c.pid[pid_iq].kp;
	ctrl->foc.pi_curr_iq.ki = mc_conf()->c.pid[pid_iq].ki;
	ctrl->foc.pi_curr_iq.ts = mc_conf()->ts;
	foc_init(&ctrl->foc);
}

void contrl_adc_update(controller_t *ctrl) {
    foc_t *foc = &ctrl->foc;
    foc->phase_angle = ctrl->encoder_angle;
    clark(ctrl->phase_curr[0], ctrl->phase_curr[1], ctrl->phase_curr[2], &ctrl->curr_albt_real.a, &ctrl->curr_albt_real.b);
    fast_sin_cos_f(foc->phase_angle, &foc->phase_sin, &foc->phase_cos);
    park(ctrl->curr_albt_real.a, ctrl->curr_albt_real.b, foc->phase_sin, foc->phase_cos, &foc->curr_dq_real.d, &foc->curr_dq_real.q);
    foc->b_openloop = (ctrl->mode_running == CTRL_MODE_OPEN);
    foc_get_svm_duty(foc);
    u16 ta = foc->duty_norm[0] * mc_conf()->pwm_half_period;
    u16 tb = foc->duty_norm[1] * mc_conf()->pwm_half_period;
    u16 tc = foc->duty_norm[2] * mc_conf()->pwm_half_period;
    pwm_update_duty(ta, tb, tc);
}